/***************************************************************************//**
 * @file MSc_FDM.h
 * @author  Marek M. Cel <marekcel@mscsim.org>
 *
 * @section LICENSE
 *
 * Copyright (C) 2013 Marek M. Cel
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 ******************************************************************************/
#ifndef MSC_FDM_H
#define MSC_FDM_H

////////////////////////////////////////////////////////////////////////////////

#include "MSc_Base.h"
#include "MSc_Ground.h"

#include <utils/MSc_DataNode.h>
#include <utils/MSc_DataRef.h>
#include <utils/MSc_IntegratorRungeKutta4.h>
#include <utils/MSc_Location.h>
#include <utils/MSc_Matrix3x3.h>
#include <utils/MSc_Quaternion.h>
#include <utils/MSc_Vector3.h>

#include <models/MSc_Aerodynamics.h>
#include <models/MSc_Controls.h>
#include <models/MSc_LandingGear.h>
#include <models/MSc_MassBalance.h>
#include <models/MSc_Propulsion.h>

////////////////////////////////////////////////////////////////////////////////

#define MSCFDM_STATE_DIMENSION 12

////////////////////////////////////////////////////////////////////////////////

/**
 * Main Flight Dynamics Model class.
 * This is interface class to use MScFDM library.
 */
class MSc_FDM : public MSc_Base
{
public:

    typedef MSc_DataRef DataRef;

    /** Aircraft crash causes. */
    enum Crash
    {
        NoCrash           = 0x0,    ///< no crash
        Collision         = 0x1,    ///< collision
        GForceOverLimit   = 0x2,    ///< G-Force over limit
        AirspeedOverLimit = 0x4,    ///< airspeed over limit
        LandingGearDmage  = 0x8     ///< landing gear damage
    };

    /** State vector (array) indices. */
    enum StateIndex
    {
        StateX = 0,     ///< index of aircraft location x-component (origin of BAS axis system) expressed in WGS axis system
        StateY,         ///< index of aircraft location y-component (origin of BAS axis system) expressed in WGS axis system
        StateZ,         ///< index of aircraft location z-component (origin of BAS axis system) expressed in WGS axis system
        StatePhi,       ///< index of aircraft attitude (rotation angle about x-axis from WGS to BAS axis system in z-y-x convention)
        StateTht,       ///< index of aircraft attitude (rotation angle about y-axis from WGS to BAS axis system in z-y-x convention)
        StatePsi,       ///< index of aircraft attitude (rotation angle about z-axis from WGS to BAS axis system in z-y-x convention)
        StateU,         ///< index of aircraft linear velocity x-componet expressed in BAS axis system
        StateV,         ///< index of aircraft linear velocity y-componet expressed in BAS axis system
        StateW,         ///< index of aircraft linear velocity z-componet expressed in BAS axis system
        StateP,         ///< index of aircraft angular velocity x-componet expressed in BAS axis system
        StateQ,         ///< index of aircraft angular velocity y-componet expressed in BAS axis system
        StateR          ///< index of aircraft angular velocity z-componet expressed in BAS axis system
    };

//    /** Flight initial conditions data struct. */
//    struct InitialConditions
//    {
//        double longitude;       ///< [rad] initial longitude
//        double latitude;        ///< [rad] initial latitude
//        double altitude;        ///< [m] initial altitude
//        double heading;         ///< [rad] initial heading
//    };

    /**
     * Creates Flight Dynamics Model (FDM) object. New created FDM object
     * has to be initialized properly before first use.
     */
    MSCFDMAPI MSc_FDM();

    /** Destructor. */
    MSCFDMAPI virtual ~MSc_FDM();

    /**
     * Loads aircraft config file and initializes flight.
     * You have to call this function before time updating.
     */
    MSCFDMAPI void initialize( std::string configFile, MSc_Ground *ground );

    /**
     * Updates FDM due to simulation time step.
     * @param timeStep simulation time step [s]
     */
    MSCFDMAPI void update( double timeStep );

private:

    /**
     * Integrator wrapping class.
     * MSc_FDM::stateDerivatives(const double*,double*) is right-hand-side function
     * for integration procedure. Because MSc_FDM::stateDerivatives(const double*,double*)
     * is private MSc_FDM::Integrator is declared friend class for MSc_FDM.
     * MSc_FDM::Integrator is declared private due to friendship with MSc_FDM
     * to avoid possible access issues. Because it is used entirely inside
     * MSc_FDM, there is no need to make it public.
     */
    class Integrator : public MSc_IntegratorRungeKutta4< MSc_FDM >
    {
    public:

        Integrator( int vectSize, MSc_FDM *obj, void (MSc_FDM::*fun)(const double *, double *) ):
            MSc_IntegratorRungeKutta4( vectSize, obj, fun )
        {}
    };

    friend class MSc_FDM::Integrator;

    MSc_DataNode *m_rootNode;   ///< data tree root node

    MSc_Aerodynamics *m_aero;   ///< aerodynamics model
    MSc_Controls     *m_ctrl;   ///< flight controls model
    MSc_LandingGear  *m_gear;   ///< landing gear model
    MSc_MassBalance  *m_mass;   ///< mass inertia model
    MSc_Propulsion   *m_prop;   ///< propulsion model

    Integrator *m_integrator;   ///< integration procedure object

    MSc_Ground *m_ground;       ///< ground intersection object

    Crash m_crash;              ///< specifies aircraft crash cause

    bool m_initialized;         ///< specifies if flight has been initialized

    double m_realTime;          ///< [s] simulation real time
    double m_timeStep;          ///< [s] simulation time step

    double m_stateVect [ MSCFDM_STATE_DIMENSION ];  ///< aircraft state vector
    double m_statePrev [ MSCFDM_STATE_DIMENSION ];  ///< aircraft state vector (previous)
    double m_derivVect [ MSCFDM_STATE_DIMENSION ];  ///< aircraft state vector derivative (for output purposes only)

    MSc_Vector3    m_loc_WGS;   ///< [m] aircraft location expressed in WGS axis system
    MSc_Quaternion m_att_WGS;   ///< [rad] aircraft attitude expressed as quaternion of rotation from WGS to BAS axis system
    MSc_Vector3    m_vel_BAS;   ///< [m/s] aircraft linear velocity vector expressed in BAS axis system
    MSc_Vector3    m_omg_BAS;   ///< [rad/s] aircraft angular velocity expressed in BAS axis system

    MSc_Location m_location;    ///< aircraft location holder

    MSc_Matrix3x3 m_T_wgs2bas;  ///< WGS to BAS rotation translation matrix
    MSc_Matrix3x3 m_T_bas2wgs;  ///< BAS to WGS rotation translation matrix
    MSc_Matrix3x3 m_T_wgs2ned;  ///< WGS to NED rotation translation matrix
    MSc_Matrix3x3 m_T_ned2wgs;  ///< NED to WGS rotation translation matrix
    MSc_Matrix3x3 m_T_ned2bas;  ///< NED to BAS rotation translation matrix
    MSc_Matrix3x3 m_T_bas2ned;  ///< BAS to NED rotation translation matrix

    /** Using this constructor is forbidden. */
    MSc_FDM( const MSc_FDM & ) : MSc_Base() {}

    /** Analyses aircraft model. */
    void analyseModel();

    /** This function is called just before time integration step. */
    void anteIntegration();

    /** This function is called just after time integration step. */
    void postIntegration();

    /**
     * Computes state vector derivatives due to given state vector.
     * @param stateVect (array) state vector
     * @param derivVect (array) resulting state vector derivative
     */
    void computeStateDeriv( const double *stateVect, double *derivVect );

    /** @param stateVect */
    void updateStateData( const double *stateVect );
};

////////////////////////////////////////////////////////////////////////////////

#endif // MSC_FDM_H
